89128c71a49943e3f47152420dbcccb14fb70536,src/org/usfirst/frc/team3641/robot/Tracking.java,Tracking,autoTarget,#,47
Before Change
visionState = Constants.SEND_REQUEST;
}
if(Math.abs(error) < 1) SmartDashboard.putBoolean("TRACKED", true);
else SmartDashboard.putBoolean("TRACKED", false);
}
After Change
}
else
{
SmartDashboard.putBoolean("TRACKED", false);
visionState = Constants.TURN_TO_TARGET;
}
}
else if(visionState == Constants.WAIT_FOR_STILL)
{
newAngle = DriveBase.getDriveDirection();
if(Math.abs(newAngle-oldAngle) > Constants.MOTION_THRESHOLD)
{
DriveBase.driveNormal(0.0, 0.0);
}
else
{
visionState = Constants.SEND_REQUEST;
}
oldAngle = newAngle;
}
else if (visionState == Constants.TURN_TO_TARGET)
{
/*
double driveOutput = -1* PILoop.smoothDrive(DriveBase.getDriveDirection(), target, false);
}
*/
double ActualCurrentHeading = DriveBase.getDriveDirection();
double error = target - ActualCurrentHeading;
SmartDashboard.putNumber("Adjusted Heading", ActualCurrentHeading);
if(error>=180)
{
error -= 360;
}
else if(error<=-180)
{
error += 360;
}
if(error>=2 || error <=1 || true)
{
if (errorRefresh > Constants.KI_UPPER_LIMIT)
{
errorRefresh = Constants.KI_UPPER_LIMIT;
}
else if (errorRefresh < Constants.KI_LOWER_LIMIT)
{
errorRefresh = Constants.KI_LOWER_LIMIT;
}
errorRefresh += error;
driveOutput = -1 * (((error * Constants.DRIVE_KP) + (errorRefresh * Constants.DRIVE_KI)));
if (driveOutput > 0)
{
driveOutput+= .1;
}
else
{
driveOutput-= .1;
}
if (Math.abs(driveOutput) > .55)
{
if (driveOutput < 0)
{
driveOutput = -.55;
}
else
{
driveOutput = .55;
}
}
DriveBase.driveNormal(0.0, driveOutput);
}
if(Math.abs(error) < 1)
{
SmartDashboard.putBoolean("TRACKED", true);
DriveBase.driveNormal(0.0, 0.0);
visionState = Constants.WAIT_FOR_STILL;
oldAngle = ActualCurrentHeading;